# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
sys.path.append("/home/avatar/Minibot/04.RobotSamples/02.CarRobot")
import inspect
from BwSensorLib import BwSensorLib
from CarRobot import CarRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 

msg = """
请选择轮子模式 :
2 : 两轮
3 : 三轮
4 : 四向轮
5 : 四麦克朗姆轮
6 : 四并橡胶轮
CTRL-C to quit
"""

class AvoidObstacle:
    def __init__(self):
        # 实例化MiniBotControlLib类，并设置为udp模式
        self.mlib = MiniBotControlLib("udp")
        # 输入ip地址和端口号
        self.ip = "10.10.100.254"
        self.port = 8899
        # 连接机器人,并设置超时时间2s
        self.mlib.Connect(self.ip, self.port, 2000)
        # 实例化BwRobotLib类，并将mlib作为参数传入
        self.robotlib = BwRobotLib(self.mlib)
        # 实例化carRobot类，并将mlib作为参数传入
        self.carRobot = CarRobot(self.mlib)
        # 实例化传感器库对象
        self.sensorlib = BwSensorLib(self.mlib)
        # 根据模块连接位置，自动加载ID配置,print_flag设为True，可打印打印当前模块信息
        self.robotlib.module_ids = self.robotlib.scanNConfigCarIds(print_flag=True)
        # 配置轮式模式 2:两轮   3:三轮   4:四向轮  5:四麦克朗姆轮  6:四并橡胶轮
        print(msg)
        self.wheel_mode = BwHelpLib.inputNum()
        # 流水灯模式 1:右转  2：左转 3：报警闪光  5：流水灯（初始化）
        self.led_mode = 5
        # 配置机器人（包括ID位置与速度模式）
        # 配置ID:ids F(4),B(11),l1(8),l2(8),l3(10),l4(11),r1(4),r2(7),r3(6),r4(6) 【TOP:9】
        self.carRobot.bindIds(self.robotlib.module_ids, self.wheel_mode)
        # 延时时间
        self.wait_time_long = 3
        self.wait_time_short = 0.1
        # 设置小车速度
        self.speed = 50
        self.carRobot.update_speed(self.speed)
 
    # 避障小车
    def avoidObstacle(self):
        for b in range(4): # 若一直没有障碍物，则小车会走一个正方形的路线（边长为小车以50的速度走3秒）
            # 走正方形的边，用时5s
            for a in range(50):
                s3 = self.sensorlib.readUltrasonicSensor() # 读取超声波数据
                if (s3 <= 30 and s3 > 0): # 若障碍物在0-30cm
                    print("前面有障碍，左转避让！！")
                    self.carRobot.turnLeft() # 小车转左
                    self.mlib.SetLEDBel(2) # 设置左灯闪烁状态
                    time.sleep(self.wait_time_long)
                    # 走正方形路线的直线时遇到障碍，则在避开障碍后重新走该直线
                    a = 0
                self.carRobot.goForward()
                self.mlib.SetLEDBel(5) # 设置流水灯状态
                time.sleep(self.wait_time_short)
            print("无障碍，右转！！")
            self.carRobot.turnRight() # 小车转右
            self.mlib.SetLEDBel(1) # 设置右灯闪烁状态
            time.sleep(self.wait_time_long)
     

if __name__ == "__main__":
    # 实例化避障小车类
    car = AvoidObstacle()    
    try:
        # 执行避障小车功能3次（走3个正方形路线）
        for i in range(3):
            car.avoidObstacle()
    except KeyboardInterrupt:
        car.carRobot.stop() # 小车停止运动
        car.mlib.SetLEDBel(3) # 设置报警灯状态
        print(" Program closed! ")
    pass
 
